#include "robot.h"
#include "tachometerui.h"

Robot::Robot(quint16 id, QString host, quint16 port, Camera *camera, QObject *parent) : SocketBase(host, port, parent)
{
    this->id = id;
    this->camera = camera;
    connect(socket, SIGNAL(readyRead()), this, SLOT(readData()));

    QTimer *timer = new QTimer(this);
    connect(timer, SIGNAL(timeout()), this, SLOT(requestPosition()));
    timer->start(100);

    timer = new QTimer(this);
    connect(timer, SIGNAL(timeout()), this, SLOT(sendPositions()));
    timer->start(100);

    randomColor();
}

void Robot::randomColor()
{
    color.setRed(rand()%255);
    color.setGreen(rand()%255);
    color.setBlue(rand()%255);
}

quint16 Robot::getId()
{
    return id;
}

QColor* Robot::getColor()
{
    return &color;
}

Kordinate* Robot::getEvalPosition()
{
    return &packetToRead.kord;
}

Kordinate* Robot::getTruePosition()
{
    return truePosition;
}

void Robot::setEvalPositionLabel(QLabel *evalPositionLabel)
{
    this->evalPositionLabel = evalPositionLabel;
}

void Robot::setTruePositionLabel(QLabel *truePositionLabel)
{
    this->truePositionLabel = truePositionLabel;
}

void Robot::requestPosition()
{
    packetToWrite.cmd = STATUS_REQUEST;
    packetToWrite.kord.objektId = id;
    writeCommand();
}

void Robot::sendDestination(quint16 x, quint16 y, quint16 theta)
{
    packetToWrite.cmd = DESTINATION_COMMAND;
    packetToWrite.kord.objektId = id;
    packetToWrite.kord.xkordinate_mm = x;
    packetToWrite.kord.ykordinate_mm = y;
    packetToWrite.kord.theta_grad = theta;
    writeCommand();
}

void Robot::sendStop()
{
    packetToWrite.cmd = STOP_COMMAND;
    writeCommand();
}

void Robot::sendGo()
{
    packetToWrite.cmd = START_COMMAND;
    writeCommand();
}

void Robot::sendPositions()
{
    packetToWrite.cmd = OBJECTS_UPDATE;
    writeCommand();

    updateTruePositionLabel();

    if (socket->state() == QAbstractSocket::ConnectedState)
    {
        QByteArray data;
        QDataStream out(&data, QIODevice::WriteOnly);
        out.setVersion(QDataStream::BigEndian);
        out.writeRawData((const char*) (camera->getPositions()), objektListeSize);
        socket->write(data);
    }
}

void Robot::updateTruePositionLabel()
{
    ObjektListe *positions = camera->getPositions();
    truePositionLabel->setText("position not found");
    for (int i = 0; i < positions->anzahlObjekte; i++) {
        if (positions->kord[i].objektId == id) {
            truePosition = &positions->kord[i];
            truePositionLabel->setText(tr("x: %1, y: %2, theta: %3")
                                       .arg(truePosition->xkordinate_mm)
                                       .arg(truePosition->ykordinate_mm)
                                       .arg(truePosition->theta_grad));
            break;
        }
    }
}

void Robot::writeCommand()
{
    if (socket->state() == QAbstractSocket::ConnectedState)
    {
        QByteArray data;
        QDataStream out(&data, QIODevice::WriteOnly);
        out.setVersion(QDataStream::BigEndian);
        out.writeRawData((const char*) (&packetToWrite), comandPkgSize);
        socket->write(data);
    }
}

void Robot::updateEvalPositionLabel()
{
    evalPositionLabel->setText(tr("x: %1, y: %2, theta: %3")
                           .arg(getEvalPosition()->xkordinate_mm)
                           .arg(getEvalPosition()->ykordinate_mm)
                           .arg(getEvalPosition()->theta_grad));
}

void Robot::readData()
{
    QDataStream in(socket);
    in.setVersion(QDataStream::BigEndian);

    if (socket->bytesAvailable() >= comandPkgSize)
    {
        in.readRawData((char*) (&packetToRead), comandPkgSize);
        if(ProzessLeitSystem::instanze->camLogCheckBox())
            logTextEdit->append(tr("x: %1 y:%2 t:%3")
                                .arg(packetToRead.kord.xkordinate_mm)
                                 .arg(packetToRead.kord.ykordinate_mm)
                                 .arg(packetToRead.kord.theta_grad));
        updateEvalPositionLabel();
    }
}

QString Robot::toString()
{
    return tr("id:%1 %2:%3").arg(id).arg(host).arg(port);
}
